#include "px4_interface.h"
#include "zmq_client.h"
#include <thread>

using std::cout; 
using std::endl;

enum TestPhase {
    HOLD = 0, 
    SEPARATE,
    ALIGN
};

TestPhase test_phase_{HOLD};

void vehicle_node(uint8_t id, float spd) {
    Px4Interface px4_interface(26539+id, 24579+id, "127.0.0.1");
    UavStatus px4_info;

    mavlink_set_position_target_local_ned_t local_setpoint{};

    local_setpoint.vx = spd;
    local_setpoint.vy = 0;
    local_setpoint.z = -5;
    local_setpoint.yaw = 0;
    local_setpoint.target_component = MAV_COMP_ID_AUTOPILOT1;
    local_setpoint.type_mask = 0b0000101111100011;
    local_setpoint.coordinate_frame = MAV_FRAME_LOCAL_NED;

    while(true) {
        if (px4_interface.updated(px4_info)) {
            if (px4_info.localPos.x > 0) {
                local_setpoint.vx = -spd;
            } else {
                local_setpoint.vx = spd;
            }
            std::cout << (int)id << " send cmd to px4, spd is " << local_setpoint.vx << std::endl;

            px4_interface.send_local_target(local_setpoint);
        }

        usleep(50 * 1000);
    }
}

int main() {

    uint8_t swarm_num = 10;
    float spd = 10;

    for (uint8_t id = 1; id <= swarm_num; id++) {
        std::cout << "test start for id " << (int)id << std::endl;

        std::thread t(vehicle_node, id, spd);
        t.detach();
    }

    char key_char{'0'};

    while (true) {
        std::cout << "input phase: " << std::endl;
        std::cin >> key_char;
        test_phase_ = (TestPhase)(atoi(&key_char));
        std::cout << "change phase to " << key_char << std::endl;
    }

}